/**
 * 
 */
package ch.unizh.ifi.ailab.ai;

import org.apache.log4j.Logger;

import ch.unizh.ifi.ailab.config.ProgramSettings;
import ch.unizh.ifi.ailab.dataTypes.Command;
import ch.unizh.ifi.ailab.dataTypes.SensorData;
import ch.unizh.ifi.ailab.dataTypes.Vector2D;


/**
 * @author hans jaeckle jaeckle@gmail.com
 * 
 */
public class SMCVectorMaxX extends SensorMotorCoupling {

	// properties
	private static ProgramSettings myProp = ProgramSettings
			.getProgramSettings();

	// speed constant for speed claculation
	private double speedConst = 2.5;

	// sicherheitsabstand 60 = ca 50cm
	private int securityDist = 10;

	// ermittelt im excell Sensor Allignment
	private double maxResultingAngle = 60.037;

	// steps from center to one extrema
	private int steeringSteps = myProp.getIntProperty("dirMax")
			- myProp.getIntProperty("dirCenter");

	// angles
	double angleS1 = Math.toRadians(myProp.getIntProperty("sensor1Angle"));

	double angleS2 = Math.toRadians(myProp.getIntProperty("sensor2Angle"));

	double angleS3 = Math.toRadians(myProp.getIntProperty("sensor3Angle"));

	double angleS4 = Math.toRadians(myProp.getIntProperty("sensor4Angle"));

	// logging
	private static Logger myLog = Logger.getLogger(AICore.class);

	public SMCVectorMaxX() {
		myLog.debug("smc: initialisiere vektor maxX smc.");
	}

	public void calculateMotionCommand(SensorData data, Command command)
			throws UnavoidableObjectException {

		Vector2D resultingVector = calculateRepulsiveForce(data);

		// speed calc:
		double newSpeed = myProp.getIntProperty("speedStop");
		newSpeed += speedConst / Math.sqrt(resultingVector.getNorm());

		if (newSpeed - command.getSpeed() >= 2) {
			newSpeed = command.getSpeed() + 2;
		}

		// (-1)!!! suitcase direction has changed after building in the new
		// speed
		// controller...
		double newDir = myProp.getIntProperty("dirCenter");
		// normalerweise muss dies gleichung noch mit *(-1) erweitert werden,
		// aber da der roboter "flasch" steuert ist das gut ohne!

		newDir += Math.toDegrees(resultingVector.getAngle()) * steeringSteps
				/ maxResultingAngle;

		// notstop oder kommando setzen:
		if (data.getIo1() < securityDist || data.getIo2() < securityDist
				|| data.getIo3() < securityDist || data.getIo4() < securityDist) {
			throw new UnavoidableObjectException("smc: panik; werte: 1:"
					+ data.getIo1() + " 2:" + data.getIo2() + " 3:"
					+ data.getIo3() + " 4:" + data.getIo4()
					+ " security distance: " + securityDist);
		} else {
			// set command
			command.setSpeed((int) Math.round(newSpeed));
			command.setDirection((int) Math.round(newDir));
		}
	}

	/**
	 * finds the shortest distance in the parameter.
	 * 
	 * @param data
	 *            data values to be checked.
	 * @return value of the shortest distance.
	 */
	private int findShortestDistance(SensorData data) {
		int dist1, dist2, dist3, dist4, shortest;
		dist1 = data.getIo1();
		dist2 = data.getIo2();
		dist3 = data.getIo3();
		dist4 = data.getIo4();

		shortest = dist1;
		if (dist2 < shortest) {
			shortest = dist2;
		}
		if (dist3 < shortest) {
			shortest = dist3;
		}
		if (dist4 < shortest) {
			shortest = dist4;
		}

		return shortest;
	}

	private Vector2D calculateRepulsiveForce(SensorData data) {
		Vector2D force = new Vector2D(0, 0);

		Vector2D forceS1 = new Vector2D(calculateSensorVector(data.getIo1(),
				angleS1));
		Vector2D forceS2 = new Vector2D(calculateSensorVector(data.getIo2(),
				angleS2));
		Vector2D forceS3 = new Vector2D(calculateSensorVector(data.getIo3(),
				angleS3));
		Vector2D forceS4 = new Vector2D(calculateSensorVector(data.getIo4(),
				angleS4));

		force.plus(forceS1);
		force.plus(forceS2);
		force.plus(forceS3);
		force.plus(forceS4);

		return force;
	}

	private Vector2D calculateSensorVector(int distance, double angle) {
		double xVal = (255 * Math.sin(angle))
				/ Math.pow((distance * Math.sin(angle)), 2);
		double yVal = (255 * Math.cos(angle))
				/ Math.pow((distance * Math.cos(angle)), 2);
		return new Vector2D(xVal, yVal);
	}
}
